package com.rockwell.sniffyhunter.model;

import java.util.Map;

import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.PickTreasureState;

public class PickTreasure extends StateObject {

	private final int MAX_RETRY_COUNT = 3;
	private int m_retryCount;
	private final static double INDEX_FORWARD_DISTANCE = 80;
	private RobotCommandSequencer m_commandSequencer;
	private Map<Integer, IDetector> m_colorDetectors;
	private IRobotSensors m_sensors;
	private ExitCode m_exitCode;
	
	private enum ExitCode {
		UNINITIALIZED,
		TREASURE_PICKED,
		TREASURE_LOST,
		TREASURE_DROPED,
	}
	
	public PickTreasure(Robot robot) {
		super(robot);
		m_colorDetectors = robot.m_colorDetectors;
		m_sensors = robot.m_sensors;
		m_commandSequencer = new RobotCommandSequencer(robot);
	}
	
	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		machineState = PickTreasureState.START;
	}

	@Override
	public boolean IsDone() {
		return machineState == PickTreasureState.DONE;
	}
	
	public IStateMachine start() {
		m_retryCount = 0;
		m_exitCode = ExitCode.UNINITIALIZED;
		machineState = PickTreasureState.LOWER_PICK_ARM;
		return machineState;
	}
	
	public boolean IsTreasurePicked() {
		return m_exitCode == ExitCode.TREASURE_PICKED;
	}
	
	public boolean IsTreasureDroped() {
		return m_exitCode == ExitCode.TREASURE_DROPED;
	}
	
	public IStateMachine LowerPickArm() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.ArmDown);
		
		if (returnVal == ReturnResult.Completed) {
			log("LowerPickArm.PickTreasure", 4);

			machineState = PickTreasureState.INDEX;
		}
		
		return machineState;
	}

	public IStateMachine Index() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveForward, INDEX_FORWARD_DISTANCE);
		
		if (returnVal == ReturnResult.Completed) {
			log("Index.PickTreasure", 4);

			machineState = PickTreasureState.PICK_UP;
		}
		
		return machineState;
	}

	public IStateMachine PickUp() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.PickTreasure);
		
		if (returnVal == ReturnResult.Completed) {
			log("PickUp.PickTreasure", 4);

			machineState = PickTreasureState.MOVE_BACK;
		}
		
		return machineState;
	}

	public IStateMachine MoveBack() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveBackward, INDEX_FORWARD_DISTANCE);
		
		if (returnVal == ReturnResult.Completed) {
			log("MoveBack.PickTreasure", 4);

			machineState = PickTreasureState.ENSURE_ARM_UP_AFTER_PICK;
		}
		
		return machineState;
	}
	
	public IStateMachine ensureArmUpAfterPick() {
		if (m_sensors.isArmUp()) {
			machineState = PickTreasureState.CHECK_PICKED;
		}
		else {
			machineState = PickTreasureState.ARM_UP_AFTER_PICK;
		}
		
		return machineState;
	}

	public IStateMachine armUpAfterPick() {
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.PickTreasure);
		
		if (returnVal == ReturnResult.Completed) {
			log("armUpAfterPick.PickTreasure", 4);
			sleep(2000);
			machineState = PickTreasureState.ENSURE_ARM_UP_AFTER_PICK;
		}
		
		return machineState;
	}
	
	public IStateMachine CheckPicked() {
		if (m_colorDetectors.get(DetectionIndex.TREASURE).isPresent()) {
			if (++m_retryCount > MAX_RETRY_COUNT) {
				log("Treasure lost.PickTreasure", 1);
				m_exitCode = ExitCode.TREASURE_LOST;
				machineState = PickTreasureState.DONE;
			}
			else {
				log("Retry pick.PickTreasure", 1);
				m_exitCode = ExitCode.TREASURE_DROPED;
				machineState = PickTreasureState.DONE;
			}
		}
		else {
			log("Treasure picked.PickTreasure", 1);
			m_exitCode = ExitCode.TREASURE_PICKED;
			machineState = PickTreasureState.DONE;
		}
		
		return machineState;
	}
}
